#include <iostream>
#include <ros/ros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/common.h>
#include <pcl/common/centroid.h>
#include <pcl_ros/transforms.h>
#include <pcl/conversions.h>
#include <pcl_conversions/pcl_conversions.h>
#include <dynamic_reconfigure/server.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include "lidar_tool/param_extinctConfig.h"
#include <ros/ros.h>

using namespace std;

Eigen::Affine3f transform0;
Eigen::Affine3f transform1;
Eigen::Affine3f transform2;
Eigen::Affine3f transform3;
static void Callbackdynamic(lidar_tool::param_extinctConfig &config)
{
    transform0 = Eigen::Affine3f::Identity();
    transform0.translation() << config.x0, config.y0, config.z0;
    transform0.rotate(Eigen::AngleAxisf(config.yaw0, Eigen::Vector3f::UnitZ()));
    transform0.rotate(Eigen::AngleAxisf(config.pitch0, Eigen::Vector3f::UnitY()));
    transform0.rotate(Eigen::AngleAxisf(config.roll0, Eigen::Vector3f::UnitX()));

    transform1 = Eigen::Affine3f::Identity();
    transform1.translation() << config.x1, config.y1, config.z1;
    transform1.rotate(Eigen::AngleAxisf(config.yaw1, Eigen::Vector3f::UnitZ()));
    transform1.rotate(Eigen::AngleAxisf(config.pitch1, Eigen::Vector3f::UnitY()));
    transform1.rotate(Eigen::AngleAxisf(config.roll1, Eigen::Vector3f::UnitX()));

    transform2 = Eigen::Affine3f::Identity();
    transform2.translation() << config.x2, config.y2, config.z2;
    transform2.rotate(Eigen::AngleAxisf(config.yaw2, Eigen::Vector3f::UnitZ()));
    transform2.rotate(Eigen::AngleAxisf(config.pitch2, Eigen::Vector3f::UnitY()));
    transform2.rotate(Eigen::AngleAxisf(config.roll2, Eigen::Vector3f::UnitX()));

    transform3 = Eigen::Affine3f::Identity();
    transform3.translation() << config.x3, config.y3, config.z3;
    transform3.rotate(Eigen::AngleAxisf(config.yaw3, Eigen::Vector3f::UnitZ()));
    transform3.rotate(Eigen::AngleAxisf(config.pitch3, Eigen::Vector3f::UnitY()));
    transform3.rotate(Eigen::AngleAxisf(config.roll3, Eigen::Vector3f::UnitX()));

    std::cout<<"transform0\n"<<transform0.matrix()<<std::endl;
    std::cout<<"transform1\n"<<transform1.matrix()<<std::endl;
    std::cout<<"transform2\n"<<transform2.matrix()<<std::endl;
    std::cout<<"transform3\n"<<transform3.matrix()<<std::endl;
}

ros::NodeHandle &get_handle()
{
    static ros::NodeHandle nh;
    return nh;
}
template <class T>
ros::Publisher &get_publisher(const std::string &topic)
{
    static std::map<std::string, ros::Publisher> publisher_map;
    if (publisher_map.find(topic) == publisher_map.end())
    {
        publisher_map[topic] = get_handle().advertise<T>(topic, 1000);
    }
    return publisher_map[topic];
}

template <typename Point>
void pubpc(std::string topic, pcl::PointCloud<Point> &pc, std::string frame = "base_link")
{
    sensor_msgs::PointCloud2 msgs;
    pcl::toROSMsg(pc, msgs);
    msgs.header.frame_id = frame;
    get_publisher<sensor_msgs::PointCloud2>(topic).publish(msgs);
}

void callback0(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr frames(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*cloud, *frames);
    pcl::transformPointCloud(*frames, *frames, transform0);
    pubpc("cloud0", *frames);
}
void callback1(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr frames(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*cloud, *frames);
    pcl::transformPointCloud(*frames, *frames, transform1);
    pubpc("cloud1", *frames);
}
void callback2(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr frames(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*cloud, *frames);
    pcl::transformPointCloud(*frames, *frames, transform2);
    pubpc("cloud2", *frames);
}
void callback3(const sensor_msgs::PointCloud2ConstPtr &cloud)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr frames(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::fromROSMsg(*cloud, *frames);
    pcl::transformPointCloud(*frames, *frames, transform3);
    pubpc("cloud3", *frames);
}


int main(int argc, char *argv[])
{
    ros::init(argc, argv, "test_node");
    dynamic_reconfigure::Server<lidar_tool::param_extinctConfig> server;
    dynamic_reconfigure::Server<lidar_tool::param_extinctConfig>::CallbackType f;
    f = boost::bind(&Callbackdynamic, _1); //绑定回调函数
    server.setCallback(f);
    ros::Subscriber sub0 = get_handle().subscribe("/pointcloud_lidar3", 1, callback0);
    ros::Subscriber sub1 = get_handle().subscribe("/pointcloud_lidar4", 1, callback1);
    ros::Subscriber sub2 = get_handle().subscribe("/pointcloud_lidar2", 1, callback2);
    ros::Subscriber sub3 = get_handle().subscribe("/pointcloud_lidar1", 1, callback3);

    ros::spin();

    return 0;
}
